function [x, P, K] = kalmfiltupd_joseph_simple(P, zObs, H, GammaMRGammaMT, fcnMTimes) %#codegen % TODO\LM: 与kalmfiltupd函数更接近，命名为kalmfiltupd_simple，说明与kalmfiltupd的区别
%KALMFILTUPD_JOSEPH 卡尔曼滤波增益计算、状态估计更新及误差协方差更新，采用约瑟夫型误差协方差更新计算式
%
% Input Arguments:
% # x: m*1列向量，状态估计更新之前的状态向量
% # P: m*m矩阵，误差协方差更新之前的误差协方差矩阵
% # zObs: n*1列向量，观测量
% # H: n*m矩阵，量测矩阵
% # GammaMRGammaMT: n*n矩阵,等价于GammaM*R*GammaMT，其中GammaM为量测噪声耦合矩阵，R为量测噪声序列协方差矩阵，GammaMT为GammaM的转置
% # fcnMTimes: 函数句柄，用于计算本函数中的矩阵乘法，默认为普通乘法
%
% Output Arguments:
% # x: m*1列向量，状态估计更新之后的状态向量
% # P: m*m矩阵，误差协方差更新之后的误差协方差矩阵
% # K: m*n矩阵,卡尔曼滤波增益矩阵
%
% References:
% # 《应用导航算法工程基础》“滤波增益矩阵的计算”“误差协方差矩阵的更新算法”

if (nargin<5) || isempty(fcnMTimes)
    fcnMTimes = @kfupddefaultmtimes;
end
K = fcnMTimes(int8(KFUpdMTimesFcnType.PHT), P, H') / (fcnMTimes(int8(KFUpdMTimesFcnType.HPHT), H, P) + GammaMRGammaMT); % REF1式12.46
x = K*zObs; % 反馈校正后x为零,简化
M = eye(size(P)) - fcnMTimes(int8(KFUpdMTimesFcnType.KH), K, H);
% 分解成两步计算
P = fcnMTimes(int8(KFUpdMTimesFcnType.MP), M, P); % 简化
end